路径预处理功能块的实现

该示例显示了 2D 切割应用程序。应使用刀子沿着 G 代码定义的路径进行切割。这个 SMC_TRAFO_GantryCutter2 变换用于根据路径的当前切线来确定刀的角度。

挑战在于刀具不能旋转得太快，否则切口将不干净。在示例项目中，通过新创建的路径预处理功能块解决了刀具角速度的限制 LimitTangentVelocity。它改变路径上的进给速率，使切线的变化速度不超过可配置的最大值

G 代码是一个带有平滑边角的简单矩形。

轨迹显示了执行结果：C 轴的速度以橙色显示，它对应于刀的旋转速度。按照规定，它限制为 45°/s。

该结构是数控应用的典型结构。G 代码在后台任务中读取，路径预处理也在此处进行。插值是在总线任务中执行的

这个 LimitTangentVelocity 功能块显示路径预处理功能块的必要步骤和状态。

基本原理是函数块读取、处理，然后写入路径元素（类型 SMC_GEOINFO) 来自输入队列 (poqDataIn) 到输出队列 (poqDataOut）。元素将从输入队列中移除。

更改进给速率的逻辑位于第 102—138 行中。辅助功能 ComputeMaxCurvature 计算给定路径元素上可能出现的最大曲率。

// Our velocity limitation comes here. This modifies an element from the
// input queue and copies the modified element to the output queue.
// Finally, the element is removed from the input queue

m_geo := pgeo^; // Copy the element
 // Note: the feature flags set in the G-Code with G38/G39 can be queried
 // by reading pgo^.dwFeatureFlags, like this:
 IF (SHR(m_geo.dwFeatureFlags, featureFlag) AND 1) = 1 THEN
     // feature is turned on

     ok := ComputeMaxCurvature(m_geo, kappa=> kappa_max);
     IF NOT ok THEN
         // Curvature cannot be computed
         m_state := STATE_ERROR;
         ErrorID := SMC_INVALID_PARAMETER;
         OnExit();
         RETURN;
     END_IF

     // Compute maximum allowed path velocity based on maximum curvature
     // and maximum allowed angular velocity
     IF kappa_max = 0 THEN
         // No curvature, no limitation necessary
         vel := m_geo.dVel;
     ELSE
         vel := m_maxAngularVelocity_rad / kappa_max;
     END_IF

     IF velMin < 0 OR vel < velMin THEN
         velMin := vel;
     END_IF

     // Set new maximum velocity for the element
     m_geo.dVel := MIN(m_geo.dVel, vel);
 ELSE
     // feature is turned off
 END_IF